from i2cport.mp_pca9685 import MpPca9685
from uwb.tag import Tag
import time
import math

# 方向控制参数
ANGLE_THRESHOLD = 15  # 允许的角度偏差
BASE_SPEED = 100  # 基础速度值
TURN_GAIN = 0.5  # 转向灵敏度
LEFT_PIN = 0
RIGHT_PIN = 1
MIN_MOVE_DISTANCE = 0.2  # 有效移动距离阈值
# 在全局参数区域新增
DIRECTION_UPDATE_INTERVAL = 1.0  # 方向更新间隔（秒）
# 在全局区域添加
global last_x, last_y, last_time
last_x = None
last_y = None
last_time =  time.time()

tag = Tag('COM5', base_distance=1.8)
pca9685 = MpPca9685(0x40, 50)


def move_car(x1,y1):
    # 添加方向推测需要的变量
    last_x, last_y = None, None
    current_angle = 0  # 初始角度设为0（正东方向）

    while True:
        distance_list = tag.get_distance()
        x, y = tag.calculate_tag_coordinates(distance_list)

        # 简化的函数调用（不再传递时间参数）
        current_angle = update_direction_info(x, y, current_angle)
        print("c_a:{}".format(current_angle))
        # 计算目标方向与当前方向的夹角
        dx = x1 - x
        dy = y1 - y
        target_angle = math.degrees(math.atan2(dy, dx))  # 目标方向角度

        # 计算角度偏差（处理超过180度的情况）
        angle_error = (target_angle - current_angle) % 360
        if angle_error > 180:
            angle_error -= 360

        # 到达目标判断
        if math.hypot(dx, dy) < 0.1:
            pca9685.command_drive_servo_car(LEFT_PIN, 0, RIGHT_PIN, 0)  # 完全停止
            break

            # 新增驱动电机函数调用
        drive_motors(angle_error)

        time.sleep(0.1)  # 控制循环频率
    time.sleep(1)
# 新增驱动函数


# 新增集成函数
def update_direction_info(current_x, current_y,current_angle):
    """集成时间判断和坐标更新的完整流程"""
    """集成时间判断和坐标更新的完整流程"""
    global last_x, last_y, last_time

    if time.time() - last_time > DIRECTION_UPDATE_INTERVAL:
        new_angle, new_time = calculate_direction_by_time(current_x, current_y,current_angle)
        # 更新全局坐标和时间
        last_x, last_y = current_x, current_y
        last_time = time.time()
        return new_angle
    return current_angle


# 新增独立方向计算函数
def calculate_direction_by_time(current_x, current_y,  current_angle):
    global last_x, last_y, last_time
    """根据时间间隔计算方向角度"""
    time_diff = time.time() - last_time
    if time_diff < 0.1 or last_x is None or last_y is None:
        return current_angle, last_time  # 保持原角度

    dx = current_x - last_x
    dy = current_y - last_y
    move_distance = math.hypot(dx, dy)

    if move_distance > MIN_MOVE_DISTANCE:
        new_angle = math.degrees(math.atan2(dy, dx))
        # 使用加权平均平滑角度变化
        return (current_angle * 0.3 + new_angle * 0.7), time.time()
    return current_angle, last_time


def drive_motors(angle_error):
    """根据角度误差驱动电机"""
    if abs(angle_error) > ANGLE_THRESHOLD:
        # 差速转向：根据角度差调整左右轮速差
        speed_diff = TURN_GAIN * angle_error
        left_speed = BASE_SPEED - speed_diff
        right_speed = BASE_SPEED + speed_diff
    else:
        left_speed = right_speed = BASE_SPEED

    # 限制速度范围
    left_speed = max(-255, min(255, left_speed))
    right_speed = max(-255, min(255, right_speed))

    # 驱动小车
    pca9685.command_drive_servo_car(
        left_pin=LEFT_PIN,
        left_speed=int(left_speed),
        right_pin=RIGHT_PIN,
        right_speed=int(right_speed)
    )


if __name__ == '__main__':
    # while True:
    #     drive_motors(0)
    #     time.sleep(0.1)
    move_car(0, 1.8)
    pca9685.command_drive_servo_car(LEFT_PIN, 0, RIGHT_PIN, 0)  # 完全停止
    time.sleep(0.1)